﻿using System.Collections;
using System.Text;
using SimpleJSON;
using ROSBridgeLib.std_msgs;

namespace ROSBridgeLib {
    namespace geometry_msgs {
        public class TransformMsg : ROSBridgeMsg {
            public HeaderMsg header;
            public Vector3Msg translation;
            public Vector4Msg rotation;
            public string frameId;
            public string childFrameId;



            public TransformMsg(JSONNode msg) {
                
                header = new HeaderMsg(msg["header"]);
                translation=new Vector3Msg(msg["transform"]["translation"]);
                rotation=new Vector4Msg(msg["transform"]["rotation"]);
                childFrameId=AddSlash(msg["child_frame_id"]);

                frameId=AddSlash(header.GetFrameId());

                              
            }
            public TransformMsg() {

                header = new HeaderMsg();
               
            }


            public override string ToYAMLString() {

               
                return "";

            } 


            string AddSlash(string s)
            {
                if (s.StartsWith("/")==false)
                {

                    s="/"+s;
                }

                return s;

            }

            /*          
            public static string GetMessageType() {
                return "geometry_msgs/Twist";
            }
            
            public Vector3Msg GetLinear() {
                return _linear;
            }

            public Vector3Msg GetAngular() {
                return _angular;
            }
            
            public override string ToString() {
                return "Twist [linear=" + _linear.ToString() + ",  angular=" + _angular.ToString() + "]";
            }
            
            public override string ToYAMLString() {
                return "{\"linear\" : " + _linear.ToYAMLString() + ", \"angular\" : " + _angular.ToYAMLString() + "}";
            } */
        }
    }
}